%% 计划在这里完成动态状态估计的多个参数组的适配，实现结果对比
clc
clear
%% 参数设置
mpc = case39;
realmpc = realcase39;
correctmpc = mpc;
load('branch.mat');
correctmpc.branch = branch_fix;

Xd = [0.006; 0.0697; 0.0531; 0.0436; 0.132; 0.05; 0.049; 0.057; 0.057; 0.031];
H = [500; 30.3; 35.8; 28.6; 26; 34.8; 26.4; 24.3; 34.5; 42];
R = zeros(length(Xd), 1); 
D = [0; 0;0 ;0; 0; 0; 0; 0; 0; 0];
% Xd=[0.06080; 0.11980; 0.18130];
% R=[0;0;0];
% H=[23.64; 6.4; 3.010];
% D=[0.0125;0.0034;0.0016];
f0 = 60;
deltt = 0.0005;
t_SW = 1;
t_FC = 1.0333;
t_max = 10;
t_response = 0;
T = 20;     % 10ms执行一次算法
load Z_measure.mat;
load X_true.mat;

w_syn=2*pi*f0; % 同步转速
%% 估计结果
X_correctest = [];
Z_correctest = [];
RMSE = [];
E_w_model = [];
E_w_real = [];
E_w_correct = [];

%% 程序运行
% 初始参数对照
[X_modelest,Z_modelest,RMSE_model,n,s] = DSE_EKF(mpc,f0,Xd,R,H,D,deltt,t_SW,t_FC,t_max,t_response,T,Z_measure);
% 真实参数对照
[X_realest,Z_realest,RMSE_real,n,s] = DSE_EKF(realmpc,f0,Xd,R,H,D,deltt,t_SW,t_FC,t_max,t_response,T,Z_measure);
% 实验
[X_correctest,Z_correctest,RMSE,n,s] = DSE_EKF(correctmpc,f0,Xd,R,H,D,deltt,t_SW,t_FC,t_max,t_response,T,Z_measure);


%% Plots
t = (0:deltt:t_max);
for i=1:1:n
    figure(i)
    subplot(3,3,1)
    plot(t,X_true(i, :), 'linewidth', 1.5)
    hold on              % 每500ms一次的计算
    plot(t(1:T:end-1), X_modelest(i, :), 'color', 'r', 'linewidth', 1);
    grid on
    ylabel(sprintf('功角_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('功角真实值与估计值 \delta', 'fontsize', 12)
    legend(sprintf('功角_{%d, 真实} ',i), sprintf('功角_{%d, EKF估计}', i)); 

    subplot(3,3,2)
    plot(t,X_true(i+n, :), 'linewidth', 1.5)
    hold on     
    plot(t(1:T:end-1), X_modelest(i+n, :),  'color', 'r', 'linewidth', 1);
    grid on
    ylabel(sprintf('转速_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('转速真实值与估计值 \omega', 'fontsize', 12)
    legend(sprintf('转速_{%d, 真实} ',i), sprintf('转速_{%d, EKF估计}', i));

    % --------------------test--------------------------
    subplot(3,3,3)
    E_w_model = [E_w_model;X_true(i+n, (1:T:end-1))-X_modelest(i+n,:)];
    plot(t(1:T:end-1),E_w_model(i,:), 'linewidth', 1)
    ylabel(sprintf('转速估计误差_{%d}',i));
    xlabel('时间(s)')
    title('转速真实值与估计值的差值 \Delta \omega')
    % plot(t,X_true(i+n, :)-X_est(i+n,:), 'linewidth', 1.5)
    % --------------------test--------------------------
end
for i=1:1:n
    figure(i)
    subplot(3,3,4)
    plot(t,X_true(i, :), 'linewidth', 1.5)
    hold on              % 每500ms一次的计算
    plot(t(1:T:end-1), X_realest(i, :), 'color', 'r', 'linewidth', 1);
    grid on
    ylabel(sprintf('功角_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('功角真实值与估计值 \delta', 'fontsize', 12)
    legend(sprintf('功角_{%d, 真实} ',i), sprintf('功角_{%d, EKF估计}', i)); 

    subplot(3,3,5)
    plot(t,X_true(i+n, :), 'linewidth', 1.5)
    hold on     
    plot(t(1:T:end-1), X_realest(i+n, :),  'color', 'r', 'linewidth', 1);
    grid on
    ylabel(sprintf('转速_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('转速真实值与估计值 \omega', 'fontsize', 12)
    legend(sprintf('转速_{%d, 真实} ',i), sprintf('转速_{%d, EKF估计}', i));

    % --------------------test--------------------------
    subplot(3,3,6)
    E_w_real = [E_w_real;X_true(i+n, (1:T:end-1))-X_realest(i+n,:)];
    plot(t(1:T:end-1),E_w_real(i,:), 'linewidth', 1)
    ylabel(sprintf('转速估计误差_{%d}',i));
    xlabel('时间(s)')
    title('转速真实值与估计值的差值 \Delta \omega')
    % plot(t,X_true(i+n, :)-X_est(i+n,:), 'linewidth', 1.5)
    % --------------------test--------------------------
end
for i=1:1:n
    figure(i)
    subplot(3,3,7)
    plot(t,X_true(i, :), 'linewidth', 1.5)
    hold on              % 每500ms一次的计算
    plot(t(1:T:end-1), X_correctest(i, :), 'color', 'r', 'linewidth', 1);
    grid on
    ylabel(sprintf('功角_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('功角真实值与估计值 \delta', 'fontsize', 12)
    legend(sprintf('功角_{%d, 真实} ',i), sprintf('功角_{%d, EKF估计}', i)); 

    subplot(3,3,8)
    plot(t,X_true(i+n, :), 'linewidth', 1.5)
    hold on     
    plot(t(1:T:end-1), X_correctest(i+n, :),  'color', 'r', 'linewidth', 1);
    grid on
    ylabel(sprintf('转速_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('转速真实值与估计值 \omega', 'fontsize', 12)
    legend(sprintf('转速_{%d, 真实} ',i), sprintf('转速_{%d, EKF估计}', i));

    % --------------------test--------------------------
    subplot(3,3,9)
    E_w_correct = [E_w_correct;X_true(i+n, (1:T:end-1))-X_correctest(i+n,:)];
    plot(t(1:T:end-1),E_w_correct(i,:), 'linewidth', 1)
    ylabel(sprintf('转速估计误差_{%d}',i));
    xlabel('时间(s)')
    title('转速真实值与估计值的差值 \Delta \omega')
    % plot(t,X_true(i+n, :)-X_est(i+n,:), 'linewidth', 1.5)
    % --------------------test--------------------------
end
% 转速估计误差
for i = 1:1:n
    figure(n+i);
    hold on;
    plot(t(1:T:end-1),E_w_model(i,:)./w_syn, 'linewidth', 1)
    plot(t(1:T:end-1),E_w_real(i,:)./w_syn, 'color', 'r','linewidth', 1)
    plot(t(1:T:end-1),E_w_correct(i,:)./w_syn, 'color', 'g','linewidth', 1)
    legend(sprintf('未校正'), sprintf('真实'), sprintf('矫正后'));
    title('转速估计误差百分比')
    xlabel('时间(s)')
    ylabel(sprintf('转速估计误差_{%d}',i));
    SNR_uc = log10(mean(X_true.^2)/mean(E_w_model(i,:).^2));
    SNR_c = log10(mean(X_true.^2)/mean(E_w_correct(i,:).^2));
    text(0.1,0.95,['矫正后信噪比/矫正前信噪比 = ',num2str(SNR_c/SNR_uc)],'Units','normalized');
end
%% 角度估计误差
% E_w_model = [E_w_model;X_true(i+n, (1:T:end-1))-X_modelest(i+n,:)];
% E_w_real = [E_w_real;X_true(i+n, (1:T:end-1))-X_realest(i+n,:)];
% E_w_correct = [E_w_correct;X_true(i+n, (1:T:end-1))-X_correctest(i+n,:)];
% for i = 1:1:n
%     figure(n+n+i);
%     hold on;
%     plot(t(1:T:end-1),E_w_model(i,:)./w_syn, 'linewidth', 1)
%     plot(t(1:T:end-1),E_w_real(i,:)./w_syn, 'color', 'r','linewidth', 1)
%     plot(t(1:T:end-1),E_w_correct(i,:)./w_syn, 'color', 'g','linewidth', 1)
%     legend(sprintf('未校正'), sprintf('真实'), sprintf('矫正后'));
%     title('转速估计误差百分比')
%     xlabel('时间(s)')
%     ylabel(sprintf('转速估计误差_{%d}',i));
%     SNR_uc = log10(mean(X_true.^2)/mean(E_w_model(i,:).^2));
%     SNR_c = log10(mean(X_true.^2)/mean(E_w_correct(i,:).^2));
%     text(0.5,0.95,['矫正后信噪比/矫正前信噪比 = ',num2str(SNR_c/SNR_uc)],'Units','normalized');
% end